"""
该脚本进行标定测试
"""
import numpy as np
import cv2
from camera_utils.camera import read_yaml
from camera_utils.config import CALIB_DIR,CALIB_MODE,CAMERA_TYPE

# 将pnp.py中得到结果填到这里
T = np.float64([0.154825101, -0.987939297, 0.002265570, 0.051264733,
-0.012027753, -0.004177973, -0.999918936, 0.005827284,
0.987868676, 0.154785300, -0.012529545, -0.084000766,
0.000000000, 0.000000000, 0.000000000, 1.000000000]).reshape(4,4)

if __name__ == "__main__":
    dir = CALIB_DIR
    ct = 1
    num = 7 # 图片数
    if CALIB_MODE == 1:
        ct = CAMERA_TYPE
    K_0,C_0= read_yaml(ct)[1:3]
    for i in range(num):

        df = np.loadtxt(f"{dir}/{i}_cols.csv",  delimiter=' ')
        df = df[:,:3].astype(np.float32)

        norm = np.linalg.norm(df,axis = 1)

        # df = df[norm > 0.4]

        rvec = cv2.Rodrigues(T[:3, :3])[0]
        tvec = T[:3, 3]

        ips = cv2.projectPoints(df, rvec, tvec, K_0, C_0)[0].reshape(-1, 2)
        if CALIB_MODE == 1:
            im = cv2.imread(f"{dir}/{ct}_c{i}.jpg")

            cn = np.loadtxt(f"{dir}/{ct}_{i}_cors.txt", delimiter=' ').reshape(-1, 3)
            cp = np.loadtxt(f"{dir}/{ct}_{i}_corner.txt", dtype=object, delimiter=' ')[:, :2].astype(
                np.float32).reshape(-1, 2)
        else:
            im = cv2.imread(f"{dir}/{ct}_c{i * 2 + ct}.jpg")

            cn = np.loadtxt(f"{dir}/{0}_{i * 2 + 0}_cors.txt", delimiter=' ').reshape(-1, 3)
            cp = np.loadtxt(f"{dir}/{ct}_{i * 2 + ct}_corner.txt", dtype=object, delimiter=' ')[:, :2].astype(
                np.float32).reshape(-1, 2)

        cp = np.array(cp,np.int)

        cn = cv2.projectPoints(cn,rvec,tvec,K_0,C_0)[0].reshape(-1,2).astype(np.int)

        for p in ips:
            cv2.circle(im, tuple(p.astype(int)), 1, (0, 255, 0), 2)


        # 绘制四边线
        cv2.line(im, tuple(cn[0]), tuple(cn[1]), (255, 255, 255), 2)
        cv2.line(im, tuple(cn[1]), tuple(cn[2]), (255, 255, 255), 2)
        cv2.line(im, tuple(cn[2]), tuple(cn[3]), (255, 255, 255), 2)
        cv2.line(im, tuple(cn[3]), tuple(cn[0]), (255, 255, 255), 2)

        cv2.line(im, tuple(cp[0].astype(np.int).tolist()), tuple(cp[1].astype(np.int).tolist()), (0, 255, 255), 2)
        cv2.line(im, tuple(cp[1].astype(np.int).tolist()), tuple(cp[2].astype(np.int).tolist()), (0, 255,255), 2)
        cv2.line(im, tuple(cp[2].astype(np.int).tolist()), tuple(cp[3].astype(np.int).tolist()), (0, 255, 255), 2)
        cv2.line(im, tuple(cp[3].astype(np.int).tolist()), tuple(cp[0].astype(np.int).tolist()), (0, 255, 255), 2)

        cv2.namedWindow("out", cv2.WINDOW_NORMAL)
        path = f"{dir}/{ct}_result{i*2+ct}.jpg" if CALIB_MODE == 2 else f"{dir}/{ct}_result{i}.jpg"
        cv2.imwrite(path,im)
        cv2.imshow("out", im)
        cv2.waitKey(0)